#include "acoTest.h"

void acoTest::G2D() {

    adjacencyMatrix.clear();
    adjacencyMatrix = std::vector<std::vector<bool>>(citys, std::vector<bool>(citys, false));

    for (int i = 0; i < citys; i++) {
        for (int j = 0; j < citys; j++) {
            if (judgeAdjacency(i, j))
                adjacencyMatrix[i][j] = true;//true为通达
            //tau[i][j] = 0.01;//初始化信息素
        }
    }

//    for (int i = 0; i < k; i++) {
//        for (int j = 0; j < k; j++) {
//            std::cout << adjacencyMatrix[i][j] << " ";
//        }
//        std::cout << std::endl;
//    }

    return;
}

bool acoTest::judgeAdjacency(int p1, int p2) {
    if (p1 == p2)
        return false;

    int x1, x2, y1, y2;

    x1 = p1 / rows;
    x2 = p2 / rows;
    y1 = p1 % rows;
    y2 = p2 % rows;

    if (fabs(x1 - x2) <= 1 && fabs(y1 - y2) <= 1 && vert[x2][y2] == 0) {
        return true;
    }

    return false;
}

bool acoTest::judgeGo(int cur, int next) {
    pair_int sta = tool::numToPair(rows, cur);
    pair_int end = tool::numToPair(rows, next);

    Direction direct = tool::judgeDirection(sta, end);
    if(direct == LOWERRIGHT) {
        if(vert[sta.second+1][sta.first] && vert[sta.second][sta.first+1]) {
            return false;
        }
    } else if(direct == LOWERLEFT) {
        if(vert[sta.second][sta.first-1] && vert[sta.second+1][sta.first]) {
            return false;
        }
    } else if(direct == UPPERLEFT) {
        if(vert[sta.second][sta.first-1] && vert[sta.second-1][sta.first]) {
            return false;
        }
    } else if(direct == UPPERRIGHT) {
        if(vert[sta.second][sta.first+1] && vert[sta.second-1][sta.first]) {
            return false;
        }
    }

    return true;
}

void acoTest::init() {
    rows = vert.size();
    rho = 0.3;//evaporation parameter，挥发参数，每次信息素要挥发的量 信息素挥发因子ρ
    alpha = 1;
    beta = 3;// alpha 和 beta分别表示pheromone(信息素)和heuristic的比重
    citys = rows * rows;
    ants = citys;
//    for (int i = 0; i < rows; i++) {
//        for (int j = 0; i < rows; j++) {
//            if (!vert[i][j]) {
//                ++ants;
//            }
//        }
//    }
    pathes = std::vector<std::vector<pair_int>>(ants, std::vector<pair_int>());
    //bestPathes = std::vector<std::vector<pair_int>>(INTERATION, std::vector<pair_int>());
    pheromone = std::vector<std::vector<double>>(citys, std::vector<double>(citys));
    heuristic = std::vector<std::vector<double>>(rows, std::vector<double>(rows));
    //信息素矩阵
    tau = std::vector<std::vector<double>>(citys, std::vector<double>(citys, 0));

    for (int i = 0; i < rows; i++) {
        for (int j = 0; j < rows; j++) {
            if (!vert[i][j]) {
                heuristic[j][i] = 1.0 / (tool::distance(std::make_pair(j, i), stop) + MINIMUM);
            }
        }
    }


    //G2D();

    return;
}

pair_int acoTest::selectNext(pair_int k) {
    double rnd = (double)(rand()) / (double)RAND_MAX;   //产生0..1的随机数

    int num = 0;
    double sum = 0.0, sums = 0.0, t;
    std::vector<pair_di> temp;              //first为T^A*H^B(pheromone ^ alpha * herustic ^ beta),second为城市序号
    int t0 = tool::pairToNum(rows, k);
    pair_int t1;
    for(int i = (t0 - rows -1)>0 ? t0-rows-1:0; i < citys; ++i) {

        t1 = tool::numToPair(rows, i);
        if(tool::judgeDirection(k, t1) != OTHER) {
            if(adjacencyMatrix[t0][i]){

                if(!judgeGo(t0, i))
                    continue;

                    ++num;
                    t = pow(tau[t0][i], alpha) * pow(heuristic[k.first][k.second], beta);

            //            if(tau[t0][i] != 0.05) {
            //                std::cout << "tau[t0][i]:" << tau[t0][i] << std::endl;
            //                std::cout << "heuristic[k.first][k.second]:" << heuristic[k.first][k.second] << std::endl;
            //            }

                    sum += t;
                    temp.push_back(std::make_pair(t, i));
                }
        }

//        if(tool::numToPair(rows, i) == k)
//            continue;
//        else if(i > t0 + rows + 1)
//            break;
//        else if(adjacencyMatrix[t0][i]){

//            if(!judgeGo(t0, i))
//                continue;

//            ++num;
//            t = pow(tau[t0][i], alpha) * pow(heuristic[k.first][k.second], beta);


//            sum += t;
//            temp.push_back(std::make_pair(t, i));
//        }
    }

    if(!num)
        return std::make_pair(-1, -1);

    //std::cout << "num:" << num << std::endl;
    rnd *= sum;
    //std::cout << "rnd:" << rnd << std::endl;
    //std::cout << "sum:" << sum << std::endl;
    for(int i=0; i<num; ++i){
        sums += temp[i].first;
        //std::cout << "sums:" << sums << std::endl;
        if(sums >= rnd){
            pair_int t1 = tool::numToPair(rows, temp[i].second);
            return t1;
        }
    }

    return std::make_pair(-1, -1);
}

void acoTest::updateD(std::vector<pair_int> passBy) {
    int t = tool::pairToNum(rows, current);
    int k = passBy.size();

    for(int i = 0; i < k; ++i){
        int t1 = tool::pairToNum(rows, passBy[i]);
        adjacencyMatrix[t][t1] = false;//false为禁忌
    }

    return;
}

void inline acoTest::updatePheromone(int j) {

    int k = pathes[j].size();
    int n1, n2;

    for (int i = 0; i < rows; i++) {
        for (int k = 0; k < rows; k++) {
            if (tau[i][k]) {
                tau[i][k] *= (1 - rho);
            }
        }
    }

    deltaTau = Q / dist;
    //std::cout << "deltaTau:" << deltaTau << std::endl;
    for(int i = 1; i < k; ++i) {
        n1 = tool::pairToNum(rows, pathes[j][i-1]);
        n2 = tool::pairToNum(rows, pathes[j][i]);
        tau[n1][n2] +=  deltaTau;
        //printf("tau[%d][%d]:%f\n",n1,n2,tau[n1][n2]);
    }

    //deltaTau = Q / tool::distance(last, current);
    //tau[n1][n2] = (1 - rho) * tau[n1][n2] + deltaTau;
}

std::vector<QPoint> acoTest::process() {

    pathes.clear();
    bestPathes.clear();
    bestPathe.clear();
    mins.clear();
    qtDraw.clear();

//    draw d(vert);
//    map = d.getImage();
//    cv::namedWindow("map", cv::WINDOW_AUTOSIZE);
//    cv::setMouseCallback( "map", tool::onMouse, 0 );

    std::cout << "依次点击出发点和终点" << std::endl;

//    while (true) {
//        cv::imshow("map", map);

//        if (tool::count == 1) {
//            start.first = tool::origin.x;
//            start.second = tool::origin.y;
//            start.first /= SCALE;
//            start.second /= SCALE;
//            cv::circle(map, cv::Point(start.first*SCALE+SCALE/2, start.second*SCALE+SCALE/2), 10, cv::Scalar(200, 100, 100), -1);
//            tool::count++;
//            printf("start:(%d, %d)\n", start.first, start.second);
//        } else if (tool::count == 3) {
//            stop.first = tool::origin.x;
//            stop.second = tool::origin.y;
//            stop.first /= SCALE;
//            stop.second /= SCALE;
//            cv::circle(map, cv::Point(stop.first*SCALE+SCALE/2, stop.second*SCALE+SCALE/2), 10, cv::Scalar(200, 100, 100), -1);

//            cv::imshow("map", map);
//            cv::waitKey(10);

//            printf("stop:(%d, %d)\n", stop.first, stop.second);
//            break;
//        }

//        cv::waitKey(10);
//    }


    printf("start:(%d, %d)\n", start.first, start.second);
    printf("stop:(%d, %d)\n", stop.first, stop.second);
    srand((int)time(0));
    init();

    min = INT32_MAX;
    minNum = 0;
    for(int i=0; i<INTERATION; ++i){

        for (int i = 0; i < citys; i++) {
            for (int j = 0; j < citys; j++) {
                if (judgeAdjacency(i, j))
                    tau[i][j] = 0.05;//初始化信息素
            }
        }

        for(int j=0; j<ants; ++j){

            G2D();
            current = start;
            last = start;
            dist = 0;

            while(current != stop){
                pathes[j].push_back(current);
                current = selectNext(current);
                if(current == std::make_pair(-1, -1)){
                    break;
                }
                //d.show(last, current);
                dist += tool::distance(last, current);
                updateD(pathes[j]);
                last = current;
            }

            if(current == stop) {
                pathes[j].push_back(stop);
                updatePheromone(j);
            } else {
                pathes[j].clear();
                continue;
            }

            if(dist < min){
                min = dist;
                minNum = j;
            }
            //cv::waitKey(0);
        }

        bestPathes.push_back(pathes[minNum]);
        mins.push_back(min);

        std::cout << "min:" << min << std::endl;
        //std::cout << "minNum:" << minNum << std::endl;
    }

    bestPathe = bestPathes[tool::findMin(mins)];

    //std::cout << "bestPathe.size():" << bestPathe.size() << std::endl;
    optimizationRoute();
    int k = bestPathe.size();

    min = 0;
    for(int i=1; i < k; ++i) {
        if(tool::judgeDirection(bestPathe[i-1], bestPathe[i])%2) {
            min += 1.414213562373;
        } else {
            min += 1;
        }
    }
    std::cout << std::endl;
    //std::cout << "bestPathe.size():" << bestPathe.size() << std::endl;

//    for(int i=0;i<bestPathe.size();i++)
//        printf("(%d,%d)\n", bestPathe[i].first, bestPathe[i].second);

    //d.show(bestPathe);

    //cv::waitKey(0);
    //std::cout << "here" << std::endl;
    qtDraw = tool::routeToPoint(bestPathe, start, scale);
    //std::cout << "here" << std::endl;
    return qtDraw;
}

//优化最终路径
void acoTest::optimizationRoute() {

//        for(int i=0;i<bestPathe.size();i++){
//            printf("(%d,%d) ",bestPathe[i].first,bestPathe[i].second);
//        }
//        std::cout << std::endl;

    int i, j, l, n, m;
    for(i = 0; i < bestPathe.size() - 2; i++) {

        for (j = i + 4; j > i + 1; j--) {

            if(j >= bestPathe.size()) {
                continue;
            }

            if(tool::judgeDirection(bestPathe[i], bestPathe[j]) != OTHER) {

                int n1 = tool::pairToNum(rows, bestPathe[i]);
                int n2 = tool::pairToNum(rows, bestPathe[j]);
                if(!judgeGo(n1, n2)) {
                    continue;
                }

                n = j - i -1;
                for(l = i + 1, m = 0; m < n; m++) {
                    bestPathe.erase(bestPathe.begin()+l);
                }
                break;
            }

        }
    }

//        for(int i=0;i<bestPathe.size();i++){
//            printf("(%d,%d) ",bestPathe[i].first,bestPathe[i].second);
//        }
//        std::cout << std::endl;

}
